#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener, Buffer
from geometry_msgs.msg import TransformStamped
from tf2_geometry_msgs import tf2_geometry_msgs  # 用于转换
from nav_msgs.msg import Odometry
class TfSubscriber(Node):
    def __init__(self):
        super().__init__('tf_subscriber')
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.timer = self.create_timer(1.0, self.timer_callback)  # 每秒查询一次
        self.publisher = self.create_publisher(Odometry, 'odom', 10)
    def timer_callback(self):
        try:
            # 获取 map 到 base_footprint 的变换
            transform = self.tf_buffer.lookup_transform(
                'map', 'base_footprint', rclpy.time.Time())
            
            # 提取位置和方向
            position = transform.transform.translation
            orientation = transform.transform.rotation
            
            self.get_logger().info(
                f'Robot position in map frame: x={position.x}, y={position.y}, z={position.z}')
            self.get_logger().info(
                f'Robot orientation in map frame: x={orientation.x}, y={orientation.y}, z={orientation.z}, w={orientation.w}')

             # 发布Odometry消息
            odom_msg = Odometry()
            odom_msg.header.stamp = self.get_clock().now().to_msg()
            self.odom_frame_id = 'odom'
            self.child_frame_id = 'base_footprint'
            odom_msg.header.frame_id = self.odom_frame_id
            odom_msg.child_frame_id = self.child_frame_id
            # odom_msg.pose.pose.position.x = position.x
            # odom_msg.pose.pose.position.y = position.y
            odom_msg.pose.pose.position.x = 0.0
            odom_msg.pose.pose.position.y = 0.0
            odom_msg.pose.pose.orientation.w = orientation.w
            odom_msg.pose.pose.orientation.z = orientation.z
            # odom_msg.twist.twist.linear.x = self.vx
            # odom_msg.twist.twist.angular.z = self.vtheta

            self.publisher.publish(odom_msg)
        except Exception as e:
            self.get_logger().error(f'Failed to get transform: {e}')

def main(args=None):
    rclpy.init(args=args)
    tf_subscriber = TfSubscriber()
    rclpy.spin(tf_subscriber)
    rclpy.shutdown()

if __name__ == '__main__':
    main()